﻿using System;
using System.Collections.Generic;
using System.Text;
using Engine;
using ProtoBuf;
using SharpDX;

namespace KDTrees
{
    [ProtoContract]
    public class MeshNode
    {
        [ProtoMember(1)]
        public Vector3 AABBMax;
        [ProtoMember(2)]
        public Vector3 AABBMin;
        /// <summary>
        /// Уровень узла
        /// </summary>
        [ProtoMember(3)]
        public int NestingLevel;
        /// <summary>
        /// Предок
        /// </summary>
        public MeshNode Parent;
        /// <summary>
        /// Список тел узла
        /// </summary>
        [ProtoMember(4)]
        public List<int> ListTrInd;
        /// <summary>
        /// Потомки
        /// </summary>
        [ProtoMember(5)]
        public List<MeshNode> Children;

        public MeshNode(MeshNode parent, Vector3 AABBMax, Vector3 AABBMin, int nestingLevel)
        {
            Parent = parent;
            this.AABBMax = AABBMax;
            this.AABBMin = AABBMin;
            NestingLevel = nestingLevel;
            ListTrInd = new List<int>();
            Children = new List<MeshNode>();
        }
    }

    [ProtoContract]
    public class MeshKDTree
    {
        [ProtoMember(1)]
        public List<Vector3> vertices = new List<Vector3> { };

        [ProtoMember(2)]
        public List<int> indices = new List<int> { };

        [ProtoMember(3)]
        private int MaxNestingLevel;

        [ProtoMember(4)]
        public MeshNode Root;

        [ProtoMember(5)]
        public int MaxLevel = 0;

        [ProtoMember(6)]
        public Vector3 BSPAABBMax = new Vector3();

        [ProtoMember(7)]
        public Vector3 BSPAABBMin = new Vector3();

        public MeshKDTree(int MaxNestingLevel, Vector3[] vertices, int[] indices)
        {
            //this.vertices = vertices;
            //this.indices = indices;

            this.MaxNestingLevel = MaxNestingLevel;

            Vector3 AA = vertices[0];
            Vector3 BB = vertices[0];

            for (int i = 0; i < indices.Length; i++)
                this.indices.Add(indices[i]);

            for (int i = 0; i < vertices.Length; i++)
            {
                this.vertices.Add(vertices[i]);
                if (vertices[i].X > AA.X) AA.X = vertices[i].X;
                if (vertices[i].Y > AA.Y) AA.Y = vertices[i].Y;
                if (vertices[i].Z > AA.Z) AA.Z = vertices[i].Z;
                if (vertices[i].X < BB.X) BB.X = vertices[i].X;
                if (vertices[i].Y < BB.Y) BB.Y = vertices[i].Y;
                if (vertices[i].Z < BB.Z) BB.Z = vertices[i].Z;
            }

            BSPAABBMax = AA;
            BSPAABBMin = BB;

            Root = new MeshNode(null, AA, BB, MaxNestingLevel);

            int Lll = indices.Length;
            for (int i = 0; i < Lll; i = i + 3)
            {
                AA = vertices[indices[i]];
                BB = vertices[indices[i]];

                for (int j = 0; j < 3; j++)
                {
                    if (vertices[indices[i + j]].X > AA.X) AA.X = vertices[indices[i + j]].X;
                    if (vertices[indices[i + j]].Y > AA.Y) AA.Y = vertices[indices[i + j]].Y;
                    if (vertices[indices[i + j]].Z > AA.Z) AA.Z = vertices[indices[i + j]].Z;
                    if (vertices[indices[i + j]].X < BB.X) BB.X = vertices[indices[i + j]].X;
                    if (vertices[indices[i + j]].Y < BB.Y) BB.Y = vertices[indices[i + j]].Y;
                    if (vertices[indices[i + j]].Z < BB.Z) BB.Z = vertices[indices[i + j]].Z;
                }
                Add(i, Root, AA, BB);
            }
        }

        private int TestTwoAABB(Vector3 AA, Vector3 BB, Vector3 AA_2, Vector3 BB_2)
        {
            if (AA.X < BB_2.X || BB.X > AA_2.X) return 0;
            if (AA.Y < BB_2.Y || BB.Y > AA_2.Y) return 0;
            if (AA.Z < BB_2.Z || BB.Z > AA_2.Z) return 0;

            if (AA.X >= AA_2.X && BB.X <= BB_2.X && AA.Y >= AA_2.Y && BB.Y <= BB_2.Y && AA.Z >= AA_2.Z && BB.Z <= BB_2.Z) return 2;
            return 1;
            //0 - нет
            //1 - частично
            //2 - _2 полностью в _
        }

        private bool Add(int TrInd, MeshNode N, Vector3 AA_2, Vector3 BB_2)
        {
            //Достигнут минимальный уровень
            if (N.NestingLevel < 1)
            {
                N.Parent.ListTrInd.Add(TrInd);
                return true;
            }

            int rez = TestTwoAABB(N.AABBMax, N.AABBMin, AA_2, BB_2);
            //Не поподает
            if (rez == 0)
                return false;
            float X = N.AABBMax.X - N.AABBMin.X;
            float Y = N.AABBMax.Y - N.AABBMin.Y;
            float Z = N.AABBMax.Z - N.AABBMin.Z;

            //Попадает частично
            if (rez == 1)
            {
                //////////////////////////////////////////////////////////////////////////
                N.Parent.ListTrInd.Add(TrInd);
                return true;
            }

            //Поподает полностью

            //Нужно создать потомков
            if (N.Children.Count < 1)
            {
                int Level = N.NestingLevel - 1;

                if (X > Math.Max(Y, Z))
                {
                    N.Children.Add(new MeshNode(N, N.AABBMax, new Vector3(N.AABBMax.X - X / 2f, N.AABBMin.Y, N.AABBMin.Z), Level));
                    N.Children.Add(new MeshNode(N, new Vector3(N.AABBMax.X - X / 2f, N.AABBMax.Y, N.AABBMax.Z), N.AABBMin, Level));
                    if ((AA_2.X + BB_2.X) / 2f > N.AABBMax.X - X / 2f)
                    {
                        if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
                        if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;
                    }
                    else
                    {
                        if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;
                        if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
                    }
                }
                else
                {
                    if (Y > Math.Max(X, Z))
                    {
                        N.Children.Add(new MeshNode(N, N.AABBMax, new DVector3(N.AABBMin.X, N.AABBMax.Y - Y / 2f, N.AABBMin.Z), Level));
                        N.Children.Add(new MeshNode(N, new DVector3(N.AABBMax.X, N.AABBMax.Y - Y / 2f, N.AABBMax.Z), N.AABBMin, Level));
                        if ((AA_2.Y + BB_2.Y) / 2f > N.AABBMax.Y - Y / 2f)
                        {
                            if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
                            if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;
                        }
                        else
                        {
                            if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;
                            if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
                        }
                    }
                    else
                    {
                        N.Children.Add(new MeshNode(N, N.AABBMax, new DVector3(N.AABBMin.X, N.AABBMin.Y, N.AABBMax.Z - Z / 2f), Level));
                        N.Children.Add(new MeshNode(N, new DVector3(N.AABBMax.X, N.AABBMax.Y, N.AABBMax.Z - Z / 2f), N.AABBMin, Level));
                        if ((AA_2.Z + BB_2.Z) / 2f > N.AABBMax.Z - Z / 2f)
                        {
                            if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
                            if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;
                        }
                        else
                        {
                            if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;
                            if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
                        }
                    }
                }
            }

            //потомки уже есть

            if (Add(TrInd, N.Children[0], AA_2, BB_2)) return true;
            if (Add(TrInd, N.Children[1], AA_2, BB_2)) return true;

            return false;
        }

        

        public bool GetIntercept(Vector3 Start, Vector3 Stop, out Vector3 OPos, out Vector3 ONorm)
        {
            OPos = new Vector3();
            ONorm = new Vector3();
            LastDistance = float.MaxValue;
            StopPosition = Stop;
            StartPosition = Start;
            TestIntercept(Root);
            OPos = StopPosition;
            ONorm = Vector3.Normalize(LastNormal);
            if (LastDistance < float.MaxValue)
            return true;
            return false;
        }

        private Vector3 LastNormal = new Vector3();
        private float LastDistance = float.MaxValue;
        private Vector3 StopPosition = new Vector3();
        private Vector3 StartPosition = new Vector3();

        private void TestIntercept(MeshNode N)
        {
            Vector3 OutPos = new Vector3();
            if (HitBoundingBox(N.AABBMin, N.AABBMax, StartPosition, StopPosition - StartPosition, out OutPos))
            {
                //////////////////////////////////////////////////////////////////////////
                if (LastDistance > (StartPosition - OutPos).Length())
                {
                    Vector3 Pos = new Vector3();
                    Vector3 Norm = new Vector3();
                    foreach (int Index in N.ListTrInd)
                    {
                        if (GetInterceptPoint(out Pos, out Norm, vertices[indices[Index + 0]], vertices[indices[Index + 1]], vertices[indices[Index + 2]], StartPosition, StopPosition))
                        {
                            float d = (StartPosition - Pos).Length();
                            if (d < LastDistance)
                            {
                                StopPosition = Pos;
                                LastNormal = Norm;
                                LastDistance = d;
                            }
                        }
                    }
                    //////////////////////////////////////////////////////////////////////////
                    if (N.Children.Count > 0)
                    {
                        TestIntercept(N.Children[0]);
                        TestIntercept(N.Children[1]);
                    }
                }
            }
        }


        // вычисление точки O пересечения прямой P1P2 и плоскости треугольника abc, лежащей внутри треугольника
        // возвращает растояние от P1 до точки пронзания вдоль вектора P1P2. 
        // Если возвращает NAN (не число) - значит такой точки нет
        // и координаты самой точки O
        /// <summary>
        /// Пересечение трингла прямой
        /// </summary>
        /// <param name="dist">растояние от P1 до точки пронзания вдоль вектора P1P2</param>
        /// <param name="O">координаты точки пересечения</param>
        /// <param name="N">нормаль</param>
        /// <param name="A">А</param>
        /// <param name="B">В</param>
        /// <param name="C">С</param>
        /// <param name="P1">Р1</param>
        /// <param name="P2">Р2</param>
        /// <returns>есть пересечение</returns>
        private bool GetInterceptPoint(out Vector3 O, out Vector3 N, Vector3 A, Vector3 B, Vector3 C, Vector3 P1, Vector3 P2)
        {
            O = new Vector3();

            Vector3 V = P1 - P2;     // Направление прямой P1P2
            float Vdist = V.Length();
            Vector3 AB = (B - A);
            Vector3 AC = (C - A);
            Vector3 BC = (C - B);
            Vector3 BA = (A - B);
            N = Vector3.Cross(AB, AC);     // Нормаль к пов-ти
            if (Vector3.Dot(N, V) < 0)
            {
                N = Vector3.Cross(AC, AB);
            }

            float vn = Vector3.Dot(V, N);          // проекция V на N
            if (vn == 0) return false;      // нет пересечения (прямая паралельна плоскости)

            float t = -Vector3.Dot((P1 - A), N / vn);
            O = P1 + V * t;

            Vector3 NAB = Vector3.Cross(N, AB);  // нормаль к плоскости NAB
            Vector3 NAC = Vector3.Cross(N, AC);  // нормаль к плоскости NAC
            Vector3 NBC = Vector3.Cross(N, BC);  // нормаль к плоскости NBC

            Vector3 AO = (O - A);
            Vector3 BO = (O - B);

            float acnab = Vector3.Dot(AC, NAB);  // проекция AC на NAB
            float aonab = Vector3.Dot(AO, NAB);  // проекция AO на NAB

            float acnac = Vector3.Dot(AB, NAC);  // проекция AB на NAC
            float aonac = Vector3.Dot(AO, NAC);  // проекция AO на NAC

            float acnbc = Vector3.Dot(BA, NBC);  // проекция BA на NBC
            float bonbc = Vector3.Dot(BO, NBC);  // проекция BO на NBC

            if (Math.Sign(acnab) == Math.Sign(aonab) && Math.Sign(acnac) == Math.Sign(aonac) && Math.Sign(acnbc) == Math.Sign(bonbc)) return t <= Vdist;

            // принадлежность сторонам треугольника
            if (Math.Sign(aonab) == 0 && AO.Length() <= AB.Length()) return t <= Vdist;
            if (Math.Sign(aonac) == 0 && AO.Length() <= AC.Length()) return t <= Vdist;
            if (Math.Sign(bonbc) == 0 && BO.Length() <= BC.Length()) return t <= Vdist;

            return false;
        }

        /// <summary>
        /// Проверка на пересечение BB лучем
        /// </summary>
        /// <param name="VminB"></param>
        /// <param name="VmaxB"></param>
        /// <param name="Vorigin"></param>
        /// <param name="Vdir"></param>
        /// <param name="Vcoord"></param>
        /// <returns></returns>
        private bool HitBoundingBox(Vector3 VminB, Vector3 VmaxB, Vector3 Vorigin, Vector3 Vdir, out Vector3 Vcoord)
        {
            Vcoord = new Vector3();
            float[] minB = new float[3] { VminB.X, VminB.Y, VminB.Z };
            float[] maxB = new float[3] { VmaxB.X, VmaxB.Y, VmaxB.Z };
            float[] origin = new float[3] { Vorigin.X, Vorigin.Y, Vorigin.Z };
            float[] dir = new float[3] { Vdir.X, Vdir.Y, Vdir.Z };
            float[] coord = new float[3] { 0, 0, 0 };
            bool inside = true;
            int[] quadrant = new int[3];
            int i;
            int whichPlane;
            float[] maxT = new float[3];
            float[] candidatePlane = new float[3];
            for (i = 0; i < 3; i++)
                if (origin[i] < minB[i])
                {
                    quadrant[i] = 1;
                    candidatePlane[i] = minB[i];
                    inside = false;
                }
                else
                    if (origin[i] > maxB[i])
                    {
                        quadrant[i] = 0;
                        candidatePlane[i] = maxB[i];
                        inside = false;
                    }
                    else quadrant[i] = 2;
            if (inside)
            {
                coord = origin;
                Vcoord = new Vector3(coord[0], coord[1], coord[2]);
                return true;
            }
            for (i = 0; i < 3; i++) if (quadrant[i] != 2 && dir[i] != 0) maxT[i] = (candidatePlane[i] - origin[i]) / dir[i];
                else maxT[i] = -1f;
            whichPlane = 0;
            for (i = 1; i < 3; i++) if (maxT[whichPlane] < maxT[i]) whichPlane = i;
            if (maxT[whichPlane] < 0) return false;
            for (i = 0; i < 3; i++)
                if (whichPlane != i)
                {
                    coord[i] = origin[i] + maxT[whichPlane] * dir[i];
                    if (coord[i] < minB[i] || coord[i] > maxB[i])
                        return false;
                }
                else
                {
                    coord[i] = candidatePlane[i];
                }
            Vcoord = new Vector3(coord[0], coord[1], coord[2]);
            return true;
        }

        /// <summary>
        /// Проверка на пересечение BB лучем
        /// </summary>
        /// <param name="VminB"></param>
        /// <param name="VmaxB"></param>
        /// <param name="Vorigin"></param>
        /// <param name="Vdir"></param>
        /// <param name="Vcoord"></param>
        /// <returns></returns>
        private bool HitBoundingBox(Vector3 VminB, Vector3 VmaxB, Vector3 Vorigin, Vector3 Vdir)
        {
            float[] minB = new float[3] { VminB.X, VminB.Y, VminB.Z };
            float[] maxB = new float[3] { VmaxB.X, VmaxB.Y, VmaxB.Z };
            float[] origin = new float[3] { Vorigin.X, Vorigin.Y, Vorigin.Z };
            float[] dir = new float[3] { Vdir.X, Vdir.Y, Vdir.Z };
            float[] coord = new float[3] { 0, 0, 0 };
            bool inside = true;
            int[] quadrant = new int[3];
            int i;
            int whichPlane;
            float[] maxT = new float[3];
            float[] candidatePlane = new float[3];
            for (i = 0; i < 3; i++)
                if (origin[i] < minB[i])
                {
                    quadrant[i] = 1;
                    candidatePlane[i] = minB[i];
                    inside = false;
                }
                else
                    if (origin[i] > maxB[i])
                    {
                        quadrant[i] = 0;
                        candidatePlane[i] = maxB[i];
                        inside = false;
                    }
                    else quadrant[i] = 2;
            if (inside)
            {
                coord = origin;
                return true;
            }
            for (i = 0; i < 3; i++) if (quadrant[i] != 2 && dir[i] != 0) maxT[i] = (candidatePlane[i] - origin[i]) / dir[i];
                else maxT[i] = -1f;
            whichPlane = 0;
            for (i = 1; i < 3; i++) if (maxT[whichPlane] < maxT[i]) whichPlane = i;
            if (maxT[whichPlane] < 0) return false;
            for (i = 0; i < 3; i++)
                if (whichPlane != i)
                {
                    coord[i] = origin[i] + maxT[whichPlane] * dir[i];
                    if (coord[i] < minB[i] || coord[i] > maxB[i])
                        return false;
                }
                else
                {
                    coord[i] = candidatePlane[i];
                }
            return true;
        }
    }
}
